#include "../common/uart.h"
#include "../common/protocol.h"
#include "../common/handles.h"

#define MAX_DATA_SIZE 10

#define STATE_WAIT_START_BYTE 0
#define STATE_WAIT_TYPE 1
#define STATE_WAIT_SEQ_NUM 2
#define STATE_WAIT_SIZE 3
#define STATE_READ_DATA 4
#define STATE_WAIT_CHECKSUM 5

extern char *buf;

void parser_start2() {
    unsigned i, j, k;

    while (1) {
        unsigned char in = (unsigned char) pRead();

        if (in == START_BYTE) {
            write(START_BYTE);

            for (i = 0; i < 10; i++) {
                write((unsigned char) pRead());
            }
        }
    }
}

void parser_start() {
    unsigned int state = STATE_WAIT_START_BYTE;
    unsigned int type = 0;
    unsigned int seq_num = 0;
    unsigned int size = 0;
    unsigned int bytes_read = 0;
    unsigned int data[MAX_DATA_SIZE];
    unsigned char checksum = 0;

    while (1) {
        unsigned int in = pRead();

        switch (state) {
            case STATE_WAIT_START_BYTE:
                if (in == START_BYTE) {
                    type = size = bytes_read = checksum = 0;
                    state = STATE_WAIT_TYPE;
                }
                break;
            case STATE_WAIT_TYPE:
                type = in;
                if (!mesg_type_exists(type)) {
                    state = STATE_WAIT_START_BYTE;
                } else {
                    checksum += type;
                    state = STATE_WAIT_SEQ_NUM;
                }
                break;
            case STATE_WAIT_SEQ_NUM:
                seq_num = in;
                checksum += seq_num;
                state = STATE_WAIT_SIZE;
                break;
            case STATE_WAIT_SIZE:
                size = in;
                checksum += size;
                if (size > MAX_DATA_SIZE) {
                    state = STATE_WAIT_START_BYTE;
                } else {
                    if (size > 0)
                        state = STATE_READ_DATA;
                    else
                        state = STATE_WAIT_CHECKSUM;
                }
                break;
            case STATE_READ_DATA:
                data[bytes_read] = in;
                checksum += in;
                bytes_read++;
                if (bytes_read == size) {
                    state = STATE_WAIT_CHECKSUM;
                }
                break;
            case STATE_WAIT_CHECKSUM:
                if (in == checksum) {
                    if (type <= MAX_VALID_HANDLE_VALUE) {
                        (*HANDLE_FUNCTIONS[type])(data, size, seq_num);
                    } else {
                        writeStr("UNKNOWN message type\r\n");
                    }
                } else {
                    writeStr("Checksum FAILED\r\n");
                }
                state = STATE_WAIT_START_BYTE;
                break;
        }
    }
}
